#ifndef __ASTAR__
#define __ASTAR__

#include "PathMap.h"
#include "Node.h"
#include "Tree.h"
#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include <iostream>
#include <ctime>
#include <stdlib.h>
#include <cstdlib>
#include <math.h>
#include <vector>
#include <fstream>
#include <stdio.h>
#include <utility>
#include <stack>
#include <queue>
#include <set>

using namespace std;

class Astar{
  public:
      //all functions are described in Astar.cpp
      Astar(float gx, float gy, SDSimulator *sim, SDSlowWalk* walkCont, SDLittleDog* littleD, float dd, Node* start_node);
      ~Astar();
      Node* runSearch();
      bool ValidStep(Node* node);
      void pushChildren(Node* current);
      bool GoalTest(Node * testNode);
      void RetracePath(Node* currentNode);
      void SavePathToFile(const char* filename);
      float getXfoot(int movingFoot, int xOff);
      float getYfoot(int movingFoot, int yOff);
      float getCostOf(Node* child);
      float dist(float* one, float* two);
      float length(float* arr);
      void SaveOutcome(Node* node, int outcome);
  
  private:  
      SDSimulator *simulator;
      SDLittleDog *littleDog;
      SDSlowWalk *walkController;
      priority_queue<Node*, vector<Node*>, smallest_f_node> Q;
      stack<step> path;
      multiset<Node*, smallest_node_node> expanded;
      multiset<Node*, smallest_node_node>::iterator it;
      Node* start;
      float goalx;
      float goaly;
      float d; //gridsize
      float stableX;
      float stableY;
      const static bool saveCosts = true;
      Tree* tree;
};


#endif


